#ifndef APF_H
#define APF_H

#include <cmath>
#include <iostream>
#include <vector>

namespace NS_APF {

struct Vec2 {
  float x;
  float y;

  Vec2() {}
  Vec2(float x, float y) : x(x), y(y) {}
  void clear() { this->x = 0, this->y = 0; }

  Vec2 operator=(const Vec2 &_v) {
    this->x = _v.x;
    this->y = _v.y;
    return *this;
  }
  Vec2 operator+(const Vec2 &_v) {
    this->x += _v.x;
    this->y += _v.y;
    return *this;
  }
  Vec2 operator-(const Vec2 &_v) {
    this->x -= _v.x;
    this->y -= _v.y;
    return *this;
  }
  Vec2 operator/(const float &_v) {
    this->x /= _v;
    this->y /= _v;
    return *this;
  }
  Vec2 operator*(const float &_v) {
    this->x *= _v;
    this->y *= _v;
    return *this;
  }
  float mod_2() { return pow(this->x, 2) + pow(this->y, 2); }
  friend std::ostream &operator<<(std::ostream &out, const Vec2 &vec2) {
    out << "[" << vec2.x << " " << vec2.y << "] ";
    return out;
  }
};

struct node {
public:
  Vec2 attractive_force_;
  Vec2 revolution_force_;
  Vec2 spring_force_;

  Vec2 sum_force_;
  Vec2 velocity_;
  Vec2 position_;
  Vec2 velocity_calculate();
  void position_update(Vec2 _pos) { position_ = _pos; }

  Vec2 attractive_force_calculate(Vec2 _object_position, float _k_a,
                                  float _goal_max_distance);
  Vec2 spring_force_calculate(Vec2 _object_position, float _k_s);
  Vec2 revolution_force_calculate(Vec2 _object_position, float _k_r,
                                  float _detect_radius);
};

struct APF_Param {
  std::vector<Vec2> obstacle_position_;
  std::vector<Vec2> init_UAV_pos_;
  float k_a_;
  float k_r_;
  float k_s_;
  float UAV_virtual_mass;
  float goal_max_distance_;
  float detect_radius_;
};

class APF {
public:
  std::vector<node> UAV_List_;
  std::vector<std::vector<float>> adjacent_;
  int fol_num_;
  APF_Param param_;
  Vec2 goal_;

  APF(APF_Param *_param_p, Vec2 _goal, int _fol_num);

  std::vector<Vec2> calculate(std::vector<Vec2> _current_position_list);
  Vec2 UAV_velocity_calculate(int _UAV_index, Vec2 _current_position);

private:
  void UAV_combine_foce_calculate(int _UAV_index);
};

} // namespace NS_APF

#endif // APF_H
